from loop_closure_f import *
import matplotlib.pyplot as plt
import math
import numpy as np
import mpl_toolkits.mplot3d #用于画3d轨迹
import scipy.io as scio
import imageio
import time
import scipy.signal as signal

# from dtw import *
import dtw
from scipy.stats import stats

from curve_line_segmentation_f import *
from pathlib import Path
from io_f import *
from compute_f import split_ts_seq, blh2xyz84, dtw_distance
from visualiz_f import *
from sample_entropy import SampEn
from pos_update import updatePos

path_data_dir = '../../数据/室内外跑车数据'
save_dir = './output/site2/F1'
# ori_filename = '/Multi_Sensor/2021-07-13-14-46-52/Orientation.txt'
# pos_filename = '/Multi_Navi/2021-07-13-14-46-52/Cal_Result.txt'
# mag_filename = '/Multi_Sensor/2021-07-13-14-46-52/MagP.txt'

# ori_filename = '/Multi_Sensor/2021-07-13-14-24-44/Orientation.txt'
# pos_filename = '/Multi_Navi/2021-07-13-14-24-44/Cal_Result.txt'
# mag_filename = '/Multi_Sensor/2021-07-13-14-24-44/Mag.txt'

ori_filename = '/Multi_Sensor/2021-12-07-14-41-56/Orientation.txt'
mag_filename = '/Multi_Sensor/2021-12-07-14-41-56/Mag.txt'
pos_filename = '/Multi_Navi/2021-12-07-14-41-56/INS_Result.txt'

ori_path_filename = path_data_dir + ori_filename
pos_path_filename = path_data_dir + pos_filename
mag_path_filename = path_data_dir + mag_filename
# path_filenames = Path(path_data_dir).resolve().glob("Orientation.txt")


if __name__ == "__main__":
    # gy = read_data_gy(ori_path_filename)
    pos = read_data_pos(pos_path_filename) #0 time, 1 lat, 2 lon, 3 h, 4 ori，5 pos_delta  10hz的输出频率
    # plt.plot(pos[:, 2],pos[:, 1], marker=',')
    # plt.show()
    mag = read_data_mag(mag_path_filename) #0 time, 1 magx, 2 magy, 3 magz, 4 mag_xyz 100hz的输出频率

    #磁补偿
    mag[:,1] = mag[:,1] - 95
    mag[:,2] = mag[:,2] - 47

    for i in range(pos.shape[0]): #BLH转XYZ
        # pos[i,1:4] = blh2xyz84(pos[i,1], pos[i,2], pos[i,3]) #0 time, 1 lat, 2 lon, 3 h, 4 ori，5 pos_delta, 6 magx, 7 magy, 8 magz, 9 mag_xyz
        pos[i,1:3] = BL2Gauss(pos[i,1] , pos[i,2])
    for i in range(pos.shape[0]-1, -1, -1): #以第一个点作为原点
        pos[i,1:4] = pos[i,1:4] - pos[0,1:4]

    pos_grid = grid_pos(pos)
    

    mag[:,0] = mag[:,0]/1000
    pos_mag = split_ts_seq(pos_grid, mag)#0 time, 1 X, 2 Y, 3 Z, 4 ori, 5 magx, 6 magy, 7 magz, 8 mag_xyz  #将位置与磁通过时间关联起来

    pos_grid = pos_mag[:,1:]# 0 X, 1 Y, 2 Z, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
    
    pos_grid = pos_grid[32500:54300,:] #第一圈旗杆室外到车库再出来到旗杆
    # pos_grid = pos_grid[55500:-1000,:] #第二圈旗杆室外到车库再出来到旗杆

    # pos_grid = pos_grid[36050:53500,:]
    # pos_grid = pos_grid[55000:,:]
    # pos_grid = pos_grid[59000:-2000,:]
    # with open("./results/01-initial.txt","w") as file_DK:
    #     for i in range(len(pos_grid)):
    #         file_DK.write('%f %f\n'%(pos_grid[i, 0], pos_grid[i, 1]))
    # file_DK.close()
    # print('01-initial.txt Write Success')
    # exit()


    
    # plt.plot(pos_grid[:, 1],pos_grid[:, 0], marker=',')
    # plt.show()
    # exit()


    # pos_grid = pos_grid[50000:,:]
    pos_grid_copy = pos_grid.copy()